import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;

public class Test {

	Pilot pilot;

	/**
	 * Moves robot in a square
	 */
	public void go() {
		float temp;
		for (int i = 0; i < 4; ++i) {
			String s;
			pilot.reset();

			pilot.travel(10f);
			s = "Distance: " + pilot.getTravelDistance();
			RConsole.println(s);
			LCD.drawString(s, 0, 2 * i);

			pilot.rotate(90f);
			temp = pilot.getAngle();
			if (temp < 0)
				temp += 360;
			s = "Angle: " + temp;
			RConsole.println(s);
			LCD.drawString(s, 0, 2 * i + 1);
		}
	}

	public static void main(String[] args) {
		RConsole.openBluetooth(1000);

		Test traveler = new Test();

		// Wheel Diameter: 3.6cm
		// Track Width: 13cm
		CompassPilot pilot = new CompassPilot(new CompassSensor(SensorPort.S3),
				3.6f, 13f, Motor.C, Motor.A);
		pilot.calibrate();

		LCD.drawString("Press true button...", 0, 0);
		Button.waitForPress();
		traveler.pilot = pilot;
		((CompassPilot)(traveler.pilot)).setSpeed(450);
		traveler.go();

		// Pause for 60 seconds with output on display
		Button.waitForPress(60000);
		RConsole.close();
	}
}
